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Abstract 

The  use  of  active  compliance  enables  robots  to  carry  out  tasks  in  the  presence 
of  significant  sensing  and  control  errors.  Compliant  motions  are  quite  difficult  for 
humans  to  specify,  however.  Furthermore,  robot  programs  are  quite  sensitive  to 
details  of  geometry  and  to  error  characteristics  and  must,  therefore,  be  constructed 
anew  for  each  task.  These  factors  motivate  the  need  for  automatic  synthesis  tools 
for  robot  programming,  especially  for  compliant  motion.  This  paper  describes  a 
formal  approach  to  the  synthesis  of  compliant  motion  strategies  from  geometric 
descriptions  of  assembly  operations  and  explicit  estimates  of  errors  in  sensing  and 
control.  A  key  aspect  of  the  approach  is  that  it  provides  correctness  criteria  for 
compliant  motion  strategies,  v" 
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1.  Introduction 


The  central  robot  programming  problem  lies  in  achieving  tasks  in  spite  of 
uncertainty  in  the  robot’s  position  relative  to  external  objects.  The  use  of  sensing 
to  reduce  uncertainty  significantly  extends  the  range  of  possible  tasks.  Sensor-based 
robot  programs  are  very  difficult  to  write,  however,  as  there  is  little  theory  to  serve 
as  a  guide.  To  make  matters  worse,  programs  written  for  one  task  are  seldom, 
if  ever,  applicable  to  other  tasks.  These  two  points  make  the  development  of  an 
automatic  synthesis  strategy  for  sensor-based  robot  programs  a  key  priority. 

In  this  paper,  we  propose  a  formal  approach  to  the  automatic  synthesis  of  a 
class  of  compliant  fine-motion  strategies  applicable  to  assembly  tasks.  The  approach 
uses  geometric  descriptions  of  parts  and  estimates  of  measurement  and  motion 
errors  to  produce  fine-motion  strategies.  Although  our  description  of  the  approach 
will  be  in  the  form  of  an  abstract  algorithm,  no  implementation  of  this  approach 
exists  at  present  (although  implementation  is  in  progress).  The  formalism  provides 
a  structured  way  of  thinking  about  fine-motion  strategies  and,  therefore,  may  be 
helpful  to  human  programmers  of  fine-motion  strategies. 

1.1.  Fine-motion  strategies 

One  important  source  of  the  difficulty  in  robot  programming  is  that  the 
programmer’s  model  of  the  environment  is  incomplete  and  inexact  as  to  the  shape 
and  location  of  objects.  Vision  may  be  used  to  determine  the  approximate  shape 
and  positions  of  objects,  but  generally  not  with  sufficient  accuracy  for  assembly 
by  pure  position  control.  Knowing  the  object  shapes  and  positions  to  sufficient 
accuracy  is  not  enough,  however.  Positioning  errors  inherently  limit  the  tasks 
achievable  by  strict  position  control.  Increasing  the  mechanical  accuracy  of  robots 
to  the  levels  required  for  assembly  is  expensive  and  ultimately  stifling.  Instead,  one 
must  abandon  the  paradigm  of  pure  position  control  for  tasks  where  the  allowable 
motions  are  tightly  constrained  by  external  objects,  as  they  are  in  mechanical 
assembly. 

The  basic  method  for  achieving  constrained  motion  in  the  presence  of  position 
uncertainty  is  by  the  use  of  controlled  compliance  (see  [Mason  83]  for  an  overview 
of  compliance  research).  Compliant  motion  meets  external  constraints  by  specifying 
how  the  robot’s  motion  should  be  modified  in  response  to  the  forces  generated 
when  the  constraints  are  violated.  Contact  with  a  surface,  for  example,  can  be 
guaranteed  by  moving  so  that  a  small  force  normal  to  the  surface  is  maintained. 
Using  this  technique,  the  robot  can  achieve  and  retain  contact  with  a  surface  that 
may  vary  significantly  in  shape  and  orientation  from  the  programmer’s  expectations. 
Generalizations  of  this  principle  can  be  used  to  accomplish  a  wide  variety  of  tasks 
involving  constrained  motion,  e.g.,  inserting  a  peg  in  a  hole  and  following  a  weld 
scam. 

The  specification  of  particular  compliant  motions  to  achieve  a  task  requires 
knowledge  of  the  geometric  constraints  imposed  by  the  task.  Given  a  description  of 
the  constraints,  choices  can  be  made  for  the  compliant  motion  parameters,  e.g.,  the 
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Figure  1.  Some  possible  initial  configurations  for  peg-in-bole  insertion 
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motion  freedoms  to  be  force  controlled  and  those  to  be  position  controlled  [Mason 
81,  Paul  and  Shimano  76,  Raibert  and  Craig  81],  or  the  center  of  compliance  and 
axis  stiffnesses  [Hanafusa  and  Asada  77,  Salisbury  80,  Whitney  83].  It  is  common, 
however,  for  position  uncertainty  to  be  large  enough  so  that  the  programmer  cannot 
unambiguously  determine  which  geometric  constraint  holds  at  any  instant  in  time. 
Figure  1,  for  example,  shows  some  different  initial,  conditions  that  can  hold  in 
two-dimensional  peg-in-hole  insertion.  Under  these  circumstances,  the  programmer 
must  employ  a  combined  strategy  of  force  and  position  control  that  guarantees 
reaching  the  desired  final  configuration  from  all  of  the  likely  initial  configurations. 
We  call  such  a  strategy  a  fine-motion  strategy . 

One  of  the  most  widely  studied  tasks  in  robotics  is  the  two-dimensional 
peg-in-hole  task.  Detailed  analyses  have  been  carried  out  to  determine  strategies 
that  guarantee  successful  insertion  once  the  peg  is  partly  in  the  hole  [Drake  77, 
McCallion  and  Wong  75,  Ohwovoriole  and  Roth  81,  Simunovic  75,  Whitney  82]. 
When  the  initial  uncertainty  in  position  is  large  enough,  a  strategy  must  also  be 
devised  to  ensure  that  the  peg  can  find  the  hole  [Inoue  74,  McCallion  and  Wong 
75].  We  can  illustrate  a  variety  of  strategies  for  one  task  by  considering  the  ways 
this  problem  has  been  addressed: 

1.  Chamfers:  Chamfers  on  the  hole  entrance  and/or  the  peg  tip  increase 
the  range  of  relative  positions  where  the  peg  can  fall  into  the  hole,  at 
least  partway.  This  technique  is  especially  effective  if  the  peg  support  has 
lateral  compliance  [Drake  77,  Whitney  82]. 

2.  Tilting  the  peg:  Tilting  the  peg  slightly  also  increases  the  range  of  relative 
positions  where  initial  entry  into  the  hole  is  guaranteed  [Inoue  74].  In  fact, 
the  geometric  effect  of  tilting  the  peg  is  almost  identical  to  providing  a 
chamfer  (see  Section  2.6). 
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3.  Search:  The  simplest  strategy  is  a  search  by  sliding  along  the  top  surface 
until  the  peg  falls  into  the  hole.  In  general,  the  search  will  have  to  pick  an 
initial  direction  of  motion  and,  possibly,  back  up  if  the  hole  is  not  found. 

4.  Biased  Search:  A  slight  modification  to  the  search  strategy  is  to  introduce 
a  bias  into  the  initial  position  of  the  peg  [Inoue  74].  This  strategy  reduces 
the  chances  of  initial  entry  into  the  hole,  but  it  guarantees  that  the  peg 
will  be  to  one  side  of  the  hole. 

In  this  paper  we  will  be  interested  in  strategies  such  as  tilting  the  peg  and 
biased  search.  These  are  simple  strategies  employing  compliant  motion  that  do  not 
require  modifying  the  task  geometry  or  complicated  control  structures. 

1.2.  Previous  work 

In  this  paper  we  present  an  approach  to  the  automatic  synthesis  of  a  class 
of  fine-motion  strategies.  We  are  aware  of  no  previous  work  with  the  same  goal. 
There  are,  however,  several  bodies  of  work  relevant  to  this  goal.  The  first  of  these 
deals  with  analyses  of  geometry  and  statics  of  tasks  so  as  to  develop  conditions 
that  successful  fine-motion  strategies  must  satisfy.  The  second  is  Simunovic's 
“information  approach”.  The  third  group  deals  with  attempts  to  derive  strategies 
starting  from  partially  specified  strategies,  known  as  skeletons  or  plans.  The  fourth 
group  deals  with  attempts  to  have  the  robot  “learn”  strategies  from  experience  and 
partial  task  information. 

Quite  a  few  authors  have  analyzed  the  peg-in-hole  assembly  task  in  detail 
[Drake  77,  Laktionev  and  Andreev  66,  Andreev  and  Laktionev  69,  Gusev  69, 
McCallion  and  Wong  75,  Ohwovoriole,  Roth,  and  Hill  81,  Ohwovoriole  and  Roth 
81,  Simunovic  75,  Whitney  82].  In  most  of  the  analyses,  the  assumption  is  that 
the  peg  is  initially  partly  in  the  hole,  possibly  at  a  chamfer.  Two  important  failure 
modes  during  insertion  have  been  identified:  jamming  and  wedging.  Jamming  is 
due  to  misproportioned  applied  forces;  wedging  is  due  to  geometric  conditions  that 
arise  when  the  parts  deform  slightly.  These  analyses  have  led  to  the  formulation  of 
conditions  for  successful  insertion  involving  applied  forces  to  relative  positions  of  the 
peg  and  hole.  As  a  result,  a  mechanical  device  (called  the  RCC  [Drake  75,  Whitney 
82])  has  been  built  that  applies  the  correct  forces  in  response  to  small  initial  errors 
between  the  peg  and  hole.  A  number  of  heuristic  strategies  for  peg- in-hole  insertion 
have  also  been  formulated,  based  on  more  fragmentary  analysis.  These  heuristic 
strategies  have  been  used  successfully  in  practice  [Inoue  74,  Goto,  Takeyasu,  and 
Inoyama  81]. 

Mason’s  [82]  detailed  analysis  of  pushing  and  grasping  operations  in  the 
presence  of  friction  also  leads  to  conditions  for  successful  task  completion.  These 
conditions  provide  the  basis  for  synthesis  of  operations  that  succeed  in  the  presence 
of  uncertainty  (without  requiring  sensing). 

Simunovic  [79]  formulated  the  "information  approach”  to  fine-motion  based 
on  the  principle  that  assembly  is  purely  a  relative  positioning  task.  From  this 
premise  he  argues  that  the  role  of  an  assembly  program  is  to  determine  the  relative 
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positions  of  parts  during  an  assembly  and  to  issue  position  commands  to  correct  the 
errors.  He  developed  an  estimation  technique  to  infer,  from  a  series  of  noisy  position 
measurements  and  using  knowledge  of  the  geometry  of  the  parts,  the  actual  relative 
positions  of  the  parts.  One  problem  with  this  approach  is  that  it  requires  a  very 
large  amount  of  on-line  computation,  although  this  could  be  solved  with  special 
purpose  electronics.  A  more  fundamental  problem  is  that  the  approach  assumes 
only  position  control  and  a  robot  capable  of  making  fine  incremental  motions.  This 
need  not  be  the  case  for  assembly;  by  exploiting  compliant  behavior  the  robot  can 
achieve  high  accuracy  tasks  even  with  low  accuracy  position  control,  for  example 
following  a  surface  by  maintaining  a  downward  force.  Another  problem  is  that 
Simunovic’s  estimation  technique  requires  knowing  which  surfaces  are  in  contact. 
This  limits  the  method  to  situations  with  relatively  small  errors;  in  more  general 
cases,  the  identity  of  the  contact  surfaces  will  not  be  known.  Our  approach  is  based 
on  a  different  view  of  assembly:  that  the  geometric  constraints  should  “guide”  the 
parts  to  their  destination  without  necessarily  having  to  know  exactly  where  the 
parts  are  relative  to  each  other. 

One  of  the  earliest  explorations  in  the  area  of  automatic  synthesis  of  fine- 
motion  strategies  from  strategy  skeletons  was  by  Taylor  [76].  Taylor  developed  a 
technique  for  propagating  the  effect  of  errors  and  uncertainties  through  a  model  of 
a  task.  These  error  estimates  were  used  to  make  decisions  for  filling-in  the  strategy 
skeletons.  For  peg-in-hole  insertion,  for  example,  the  decision  whether  to  tap  the 
peg  against  the  surface  next  to  the  hole  was  based  on  whether  the  error  estimate 
for  position  normal  to  the  surface  exceeded  a  threshold. 

Lozano-Perez  [76]  also  proposed  a  method  for  selecting  the  motion  parameters 
in  strategy  skeletons.  Each  motion  in  a  skeleton  was  specified  symbolically  by  the 
relationship  among  parts  that  it  was  designed  to  achieve.  The  expected  length  of 
guarded  moves  and  their  force  terminating  conditions  were  then  computed  from 
the  ranges  of  displacements  that  achieved  this  relationship  (taking  into  account 
uncertainty  in  position). 

Recently,  Brooks  [82]  extended  Taylor’s  approach  by  making  more  complete 
use  of  symbolic  constraints  in  the  error  computations.  The  resulting  constraints  can 
be  used  in  the  “forward”  direction  to  estimate  errors  for  particular  operations.  But, 
importantly,  they  also  may  be  used  in  the  “backward”  direction  to  constrain  the 
values  for  plan  parameters,  such  as  initial  positions  of  objects,  to  those  that  enable 
the  plan  to  succeed.  When  no  good  choice  of  parameters  exists,  the  system  chooses 
appropriate  sensing  operations  (such  as  visual  location  of  parts)  that  reduce  the 
uncertainty  enough  to  guarantee  success. 

Another  line  of  research  has  focused  on  building  up  programs  automatically 
from  attempts  by  the  robot  to  carry  out  the  operations.  Dufay  and  Latombe  [83] 
describe  how  partial  local  strategies  (“rules”)  for  a  task  can  be  assembled  into  a 
complete  program  by  processing  the  execution  traces  of  many  attempts  to  carry  out 
the  task.  The  method,  however,  requires  knowing  the  actual  relationship  between 
parts  achieved  by  each  motion,  e.g.,  which  surfaces  are  in  contact.  This  information 
can  be  obtained,  in  many  cases,  from  careful  analysis  of  the  forces  and  positions 
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Figure  2.  Variations  of  peg-in-hole  require  different  strategies 
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but,  in  general,  the  information  is  ambiguous  in  the  presence  of  measurement  and 
control  errors.  Moreover,  the  rules  used  by  the  system  are  specific  to  tasks  and 
must  be  provided  by  the  user. 

A  related  approach  to  deriving  a  strategy  from  “experiments”  is  based  on  the 
theory  of  stochastic  automata  [Simons,  et  al.  82].  The  goal  is  to  have  the  robot  learn 
the  appropriate  control  response  to  measured  force  vectors  during  task  execution. 
The  method  requires  a  task-dependent  evaluation  function  so  as  to  judge  progress 
towards  its  goal. 

These  previous  approaches  to  fine-motion  synthesis  are  based  on  the  assumption 
that  there  is  a  basic  repertoire  of  operations,  such  as  peg-in-hole  insertion  and 
block-in-corner,  whose  geometric  structure  is  known  a  priori.  In  this  view,  the  task 
of  a  synthesis  program  is  to  make  some  pre-defined  set  of  choices  among  alternative 
actions,  select  the  values  of  some  parameters,  and,  possibly,  select  the  order  of 
operations.  In  fact,  small  changes  in  the  geometry  of  parts  can  have  significant 
impact  on  fine-motion  strategies.  The  different  operations  shown  in  Figure  2,  for 
example,  can  all  be  classified  as  peg-in-hole  and,  yet,  they  require  substantially 
different  programs  to  insure  reliable  execution.  Similarly,  differences  in  expected 
position  errors  will  call  for  different  strategies  for  the  same  task. 

Our  approach  is  motivated  by  the  belief  that  the  set  of  possible  geometric 
interactions  in  a  task  should  directly  determine  the  structure  of  the  fine-motion 
strategy  for  the  task.  Thus,  for  example,  the  presence  of  additional  surfaces  within 
the  region  of  possible  initial  contact  typically  requires  a  change  in  the  structure  of  a 
strategy.  The  approach  we  describe  in  this  paper  proceeds  directly  from  geometric 
descriptions  of  the  parts  to  a  strategy. 
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Figure  3.  Peg-in-hole:  (a)  original  formulation  (b)  transformed  to  point  problem 
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2.  Overview  of  the  approach 


In  this  section  we  informally  outline  our  approach  to  fine-motion  synthesis 
using  a  progression  of  simple  examples.  In  section  3,  we  provide  a  more  formed 
characterization  of  the  approach. 

2.1.  The  basic  strategy 

Consider  the  simple  task  of  moving  the  point  p  from  its  initial  position  to 
any  one  of  the  positions  in  G  (see  Figure  3(b)).  This  is  a  simplified  problem  but 
not  a  completely  artificial  one.  It  is  equivalent  to  the  two-dimensional  peg-in-hole 
problem  in  Figure  3(a)  when  the  axes  of  the  peg  and  hole  are  constrained  to  be 
parallel.  The  position  of  p  determines  the  position  of  the  peg.  The  boundary  of 
the  shaded  area  represents  the  positions  of  p  where  the  peg  would  be  in  contact 
with  an  obstacle.  The  transformation  from  Figure  3(a)  to  Figure  3(b)  corresponds 
to  shrinking  the  peg  to  a  point  and  expanding  the  obstacles  accordingly.  Note  that 
the  sides  of  the  hole  have  each  been  moved  towards  each  other  by  half  the  width 
of  the  peg.  In  this  case,  the  transformation  produces  an  equivalent  problem.  We 
postpone  a  more  general  discussion  of  this  type  of  transformation  until  section  2.6. 
It  is  the  case  that  problems  of  moving  rigid  objects  among  other  rigid  objects  can  be 
reformulated  as  equivalent  problems  of  moving  a  point  among  transformed  objects 
in  a  higher  dimensional  space,  called  the  configuration  space  [Lozano-P6rez  81, 
83). 

The  basic  step  in  our  synthesis  approach  is  to  identify  ranges  of  positions  from 
where  p  can  reach  G  by  a  single  motion.  The  directions  of  such  motions  can  be 
represented  as  unit  velocity  vectors,  v*.  For  each  v*,  we  can  compute  all  those 
positions,  Px,  such  that  a  motion  along  Vj  from  that  position  would  reach  some 
point  of  G  (see  Figure  4).  We  call  this  range  of  positions  that  can  reach  the  goal 
by  a  single  motion  along  a  specified  velocity  the  pre-image 1  of  the  goal  (for  that 
velocity).  All  we  need  do  to  guarantee  that  p  reaches  G  from  any  point  in  any  of 
the  Px  is  to  execute  a  motion  with  commanded  velocity  along  v*. 

‘The  rationale  for  this  name  stems  from  viewing  motions  as  mappings  from  pairs  of  initial 
positions  and  velocities  into  points  along  the  resulting  path. 
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Figure  4.  Pre- image  of  the  goal  for  different  v*. 
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If  no  pre-image  of  G  contains  the  peg’s  current  position,  then  we  can  apply 
the  same  pre-image  computation  recursively  using  each  of  the  existing  pre-images 
as  a  possible  goal.  This  recursive  process  is  an  instance  of  the  problem  solving 
strategy  known  as  backward  chaining  [Nilsson  80].  Each  pre-image  of  G,  Pt,  serves 
to  define  a  new  goal  set  Gj  (the  superscript  indicates  the  “recursion  level”).  This 
process  is  repeated  until  some  pre-image  Pf  contains  the  current  position  of  p  (see 
Figure  5).  From  this  chain  of  pre-images  PiQ)P}l  ■■■, P*t  we  can  construct  a  motion 
strategy.  The  two  components  of  the  strategy  are  a  sequence  of  velocity  vectors  and 
a  sequence  of  associated  termination  predicates.  Therefore,  the  strategies  may  be 
construed  as  a  sequence  of  guarded  motions  (Will  and  Grossman  75].  Each  velocity 
vector  v».  defines  a  motion  that  moves  from  anywhere  in  Pj  to  G*  .  Whenever  p 
reaches  one  of  the  goal  regions,  the  velocity  command  needs  to  be  changed  to  that 
appropriate  for  the  new  region,  e.g.,  from  to  v^_,.  The  role  of  the  termination 
predicates  is  to  detect  the  arrival  of  p  into  a  goal  region.  In  the  simple  case  we  have 
been  discussing,  termination  predicates  simply  test  to  see  whether  the  position  is 
in  the  goal  region.  Termination  predicates  are  much  more  difficult  to  construct  in 
the  presence  of  position  uncertainty.  We  will  discuss  this  issue  further  in  section 
2.2  and  section  3. 

In  summary,  our  basic  approach  to  fine-motion  synthesis  is  to  chain  backwards 
from  the  goal  towards  the  current  position,  characterizing  at  each  step  the  range 
of  positions  that  can  reach  the  current  goal  in  one  motion,  i.e.,  the  pre-image 
of  the  goal.  It  remains  to  show  how  this  simple  approach  is  applicable  to  more 
realistic  assembly  problems.  Our  first  step  towards  this  goal  will  be  to  discuss  the 
role  played  by  uncertainty  in  position  and  velocity.  The  second  step  will  be  to 
introduce  compliant  motions.  The  next  step  will  be  to  illustrate  how  friction  can 
be  handled.  The  last  step  is  to  show  how  the  notion  of  configuration  space  reduces 
assembly  problems  for  solids  into  problems  involving  a  point  and  surfaces  in  a 
higher-dimensional  space. 

2.2.  The  effect  of  uncertainty 

We  have  assumed  thus  far  that  p’s  position  is  known  exactly  at  all  times  and 
that  its  direction  of  motion  can  be  specified  exactly.  In  this  section  we  explore  the 
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Figure  5.  Backward  chaining  of  pre-images 


Figure  6.  Weak  and  strong  pre-images  of  a  goal  under  velocity  uncertainty 


effects  of  relaxing  these  assumptions. 

Let  us  assume  that  there  is  error  between  the  actual  and  the  commanded 
velocity,  bounded  by  ev.  The  actual  velocity  is  within  a  ball  of  radius  e„  in  velocity 
space  (the  ball  of  velocities  centered  on  v  is  denoted  B{v)).  Therefore,  the  path 
of  p  is  constrained  to  be  within  a  semi-infinite  cone  centered  on  the  commanded 
path  and  whose  apex  is  the  initial  position.  The  angle  between  the  actual  direction 
of  motion  and  the  commanded  direction  is  constrained  to  be  less  than  or  equal  to 
sin~1eV)  which  will  be  approximately  e„  for  small  enough  e„. 

The  synthesis  approach  above  is  based  on  computing  the  pre-images  of  goal 
regions  for  particular  values  of  commanded  velocity.  These  are  locations  from  which 
the  goal  can  be  reached  by  a  single  motion.  In  the  presence  of  uncertainty  in  the 
actual  velocity,  we  define  two  alternative  pre-images  (see  Figure  6): 

1.  Weak  pre-image  -  locations  for  which  some  motion  within  the  range  of 
velocity  uncertainty  may  reach  the  goal. 

2.  Strong  pre-image  -  locations  for  which  all  motions  within  the  range  of 
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Figure  7.  Positions  reachable  by  commanded  motion  with  uncertainty 


velocity  uncertainty  will  reach  the  goal. 

Note  that  the  strong  pre-image  is  a  subset  of  the  weak  pre-image.  In  what 
follows,  we  will  use  the  term  pre-image  to  mean  strong  pre-image. 

In  addition  to  uncertainty  in  the  actual  velocity  along  a  motion,  there  is 
uncertainty  in  the  position  of  p.  One  source  of  position  uncertainty  is  due  to 
imperfect  knowledge  of  the  initial  position  of  the  objects  in  the  workspace.  Another 
source  of  error  is  due  to  inherent  limitations  in  the  robot’s  position  sensors.  For  the 
sake  of  simplicity,  we  will  lump  these  two  types  of  uncertainty  into  a  single  upper 
bound  on  position  uncertainty.  This  assumption  does  not  affect  correctness  of  any 
derived  motion  strategies,  but  might  lead  to  less  efficient  strategies.  In  practice,  the 
two  sources  of  uncertainty  should  be  treated  differently. 

We  assume  that  the  actual  position  is  always  within  a  ball  of  radius  ec  centered 
at  the  position  observed  by  the  robot.  All  possible  observed  positions  are  within  a 
similar  ball  centered  at  the  actual  position.  The  ball  of  possible  observed  positions 
centered  at  a  position  p  is  denoted  B(p).  The  range  of  positions  potentially  traversed 
by  a  motion  from  an  observed  position  along  a  commanded  velocity  is  depicted  in 
Figure  7. 

Position  uncertainty  makes  it  difficult  to  define  termination  predicates  for 
motions.  A  predicate  that  simply  compares  the  observed  position  of  p  against  the 
boundaries  of  Gx  could  terminate  a  motion  prematurely.  The  actual  position  of  p 
could  be  anywhere  within  a  ball  of  radius  ec  from  the  observed  position.  In  order  to 
guarantee  success  all  possible  positions  of  p  must  be  within  the  goal.  We  can  think 
of  this  effect  of  position  uncertainty  as  “shrinking”  the  goal  by  ec  for  purposes  of 
detecting  entry.  Shrinking  Gx  removes  from  G'  any  point  at  a  distance  less  than 
or  equal  to  ec  from  any  point  in  free  space  not  in  Gx.  This  removes  from  Gx  any 
point  that  is  ambiguous.  In  many  cases,  this  means  that  no  part  of  the  goal  is 
unambiguously  identifiable  on  the  basis  of  position.  We  will  have  to  rely  on  the 
effects  of  collisions  with  surfaces  or  on  previous  history  to  identify  entry  into  a  goal 
region.  This  issue  is  quite  subtle;  it  is  the  subject  of  section  3.1  (also  see  section 
2.5). 
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2.3.  Compliant  Motion 

The  example  above  dealt  only  with  position  controlled  motions.  Due  to 
uncertainty  in  p’s  position  and  velocity  relative  to  the  task,  this  type  of  motion 
often  leads  to  empty  pre-images.  This  indicates  that  the  position  accuracy  is  not 
sufficient  for  the  task.  We  mentioned  earlier  that  the  alternative  motion  regime  is 
compliant  motion.  We  can  visualize  the  effect  of  compliant  motions  as  producing 
sliding  on  the  constraint  surfaces  derived  from  the  obstacles.  Sliding  means  that 
the  moving  object  confines  its  motions  to  be  tangent  to  the  constraining  surface(s) 

[Mason  81).  When  not  in  contact  with  a  surface,  the  motion  will  _ ;g  the 

commanded  velocity  (to  within  the  velocity  uncertainty). 

The  generalized  damping  model  [Whitney  76]  can  be  used  to  implement 
compliant  motions  with  the  properties  described  above.  The  desired  motion  is 
determined  by  the  following  relationship 

f  =  B(v  —  vo) 

where  f  is  the  vector  of  forces  acting  on  the  moving  object,  vo  is  the  nominal 
velocity  vector,  and  v  is  the  actual  velocity  vector.  In  what  follows,  B  is  a  diagonal 
matrix.  The  role  of  B  in  our  usage  is  primarily  to  relate  the  units  of  force  to  those  of 
velocity.  We  assume  that  the  control  system  ensures  that  the  dynamics  of  the  robot 
and  moving  object  can  be  adequately  approximated  as  a  damper  (for  some  limited 
range  of  operating  velocities).  The  Appendix  provides  a  more  detailed  treatment  of 
the  behavior  of  a  generalized  damper. 

In  practice,  because  of  measuring  and  implementation  errors,  there  will  be 
a  difference  between  the  commanded  behavior  and  the  actual  behavior  of  the 
damper.  We  summarize  these  differences  by  introducing  the  distinction  between  the 
actual  nominal  velocity,  v0,  and  the  commanded  nominal  velocity,  denoted  v0. 
Throughout  the  paper,  the  asterisk  will  denote  measured  or  commanded  quantities 
that  differ  from  the  actual  ones  because  of  the  presence  of  error. 

The  definition  of  the  pre-image  of  a  goal  as  the  set  of  positions  that  can  reach 
the  goal  with  one  commanded  velocity  can  be  retained  for  generalized  damper 
motions.  Under  compliant  motion,  however,  the  moving  object  may  reach  the 
goal  indirectly  by  sliding  on  intervening  surfaces.  Therefore,  compliant  motions 
typically  produce  larger  pre-images  than  pure  positioning  motions.  The  increased 
pre-image  indicates  less  rensitivity  to  uncertainty  (compare  Figures  8(a)  and  8(b)). 

2.4.  Friction 

A  crucial  consideration  in  the  analysis  and  synthesis  of  fine-mowon  strategies 
is  the  effect  of  friction.  A  simple  model  of  friction  for  planar  motion  without 
rotation  is  as  follows.  We  assume  that  the  objects  are  of  a  single  material  with 
equal  coefficients  of  static  and  sliding  friction,  p.  The  reaction  force  from  contact 
at  a  point  on  a  surface  will  lie  within  a  friction  cone  with  apex  at  the  point  of 
contact  and  center  line  along  the  surface  normal.  The  angle  between  the  normal 
and  the  sides  of  the  cone  is  the  friction  angle,  <f>  =  tan-1  p.  If  the  applied  force 
points  into  the  friction  cone,  i.e.,  if  the  angle  of  the  force  vector  to  the  surface 
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Figure  8.  Pre-images  for  position  control  vs.  generalized  damper 
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Figure  9.  Friction  cone 
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normal  is  less  than  <t>,  then  no  motion  will  result.  If  the  angle  of  the  force  vector  to 
surface  normal  is  greater  than  <j>  sliding  will  result  (see  Figure  9). 

This  model  of  friction  can  be  extended  to  include  rotations  and  moments,  but 
the  details  are  beyond  the  scope  of  this  discussion  (see  [Erdmann  83]).  In  what 
follows,  we  need  only  assume  that  some  nominal  velocity  vectors  will  cause  sticking 
on  a  surface  and  others  will  cause  sliding.  We  assume,  furthermore,  that  the  range 
of  nominal  velocity  vectors  that  cause  sticking  for  a  surface  can  be  conservatively 
bounded  by  a  cone.  The  computation  of  pre-images  must  take  into  account  the 
possibility  of  “sticking”  on  a  surface.  In  particular,  assuming  the  motion  is  generated 
by  a  damper  (with  B  =  61),  if  the  range  of  nominal  velocities  for  the  pre-image 
contains  nominal  velocities  whose  angle  to  the  normal  of  some  surface  is  less  than 
the  friction  angle,  <f>,  then  the  motion  will  stop  at  that  surface  (see  Figure  9  and 
the  Appendix). 

2.5.  Examples 

We  now  have  the  conceptual  tools  necessary  to  synthesize  a  strategy  for  the 
simple  example  of  Figure  3.  In  this  section  we  illustrate  one  particular  approach 
to  synthesis  of  strategies  based  on  the  use  of  pre-images.  The  method  used  in  this 
section  is  a  subset  of  the  general  approach  described  in  Section  3. 

Our  goal  is  to  identify  some  sub-region  P  of  the  free  space  C  and  a  command 
nominal  velocity  vo  such  that  P  is  the  strong  pre-image,  under  vo,  of  the  goal 
surface.  Equivalently,  P  must  not  overlap  the  weak  pre-image  of  any  surface  where 
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motion  along  Vo  may  stick  or  not  reach  the  goal.  We  will  use  this  constraint  to 
drive  a  simultaneous  search  for  P  and  Vo- 

The  key  problem  is  in  discovering  Vo.  Our  approach  here  is  to  narrow  in  on 
feasible  values  of  vo  by  progressive  refinement.  We  start  with  the  complete  range 
of  possible  vo’s  and  remove  from  that  range  any  values  that  can  possibly  lead 
to  failure  (by  sticking  or  not  reaching  the  goal).  At  each  step  of  the  algorithm, 
we  compute  the  strong  pre-image  of  the  goal  for  the  current  range  of  vo’s.  The 
strong  pre-image  for  a  range  of  command  velocities  is  the  intersection  of  the  strong 
pre-images  for  each  of  the  velocities.  These  are  the  positions  guarant'''”!  to  reach 
the  goal  for  all  the  velocities  in  the  range2.  This  is  the  same  definition  that  we  saw 
in  section  2.2  for  the  strong  pre-image  in  the  presence  of  velocity  error.  In  fact,  as 
long  as  the  velocity  ranges  used  to  compute  pre-images  are  greater  than  2ev,  we 
need  not  concern  ourselves  further  with  velocity  uncertainty.  Once  the  algorithm 
has  chosen  a  final  velocity  range,  we  can  pick  a  specific  velocity  from  the  range 
such  that  all  velocities  within  the  velocity  error  fall  in  the  chosen  velocity  range. 
Narrower  velocity  ranges  will  not  yield  such  a  safe  velocity. 

For  now  we  will  ignore  the  need  for  backward  chaining  and  sketch  an  algorithm 
for  synthesizing  single  motions.  We  will  deal  with  backward  chaining  presently. 
The  basic  algorithm  steps  are  as  follows: 

1.  Compute  P,  the  strong  pre-image  of  the  goal  surface,  for  the  current 
range  of  commanded  velocities.  If  the  current  range  of  velocities  is  split 
into  disjoint  sub-ranges,  then  Steps  1  and  2  should  be  repeated  for  each 
sub-range  (see  Figure  10). 

2.  If  P  includes  an  uncertainty  ball  centered  at  some  starting  position,  then 
return  P  and  the  current  velocity  range. 

3.  Pick  x  to  be  a  surface  (other  than  a  goal)  where  the  robot  may  “stick”, 
i.e.  such  that  some  velocity  in  the  current  velocity  range  points  into  the 
surface’s  friction  cone.  If  no  such  surface  exists  then  stop. 

4.  Remove  from  the  range  of  commanded  velocities  any  velocity  pointing 
into  the  friction  cone  of  x. 

5.  Go  to  Step  1. 

We  can  illustrate  the  operation  of  this  algorithm  on  our  example  as  follows. 
Construct  a  directed  graph  with  nodes  for  each  of  the  surfaces  in  Figure  3  and  one 
node  representing  free  space  (C).  A  link  is  directed  from  node  m  to  node  n  in  the 
graph  if  m  and  n  are  direct  neighbors  and  m  is  in  the  weak  pre-image  of  n  for 
the  specified  velocity  range.  That  is,  there  is  a  link  from  m  to  n  if  some  velocity 
in  the  current  range  may  cause  the  robot  to  move  from  some  point  in  m  (which  is 
not  in  n)  to  some  point  in  n  (which  is  allowed  to  be  at  the  intersection  of  m  and 
n)  without  going  through  points  in  any  other  node.  In  principle,  the  graph  should 
have  nodes  representing  the  vertices;  we  have  left  them  out  for  simplicity.  This 

2 Note  that  the  weak  pre-image  for  a  range  of  velocities  ia  the  union  of  the  weak  pre- images  for 
velocities  in  the  range,  i.e.,  positions  that  may  reach  the  goal  for  some  velocity  iD  the  range. 
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simplification  introduces  the  need  for  the  phrases  in  parentheses.  See,  for  example, 
the  link  between  A  and  D  and  B  and  G  (but  not  viceversa)  in  the  first  example. 
We  will  call  this  the  reachability  graph  for  that  range  of  commanded  velocities. 
The  reachability  graph  plays  a  key  role  in  algorithms  for  computing  the  strong 
pre-image  of  the  goal. 

In  our  example,  we  start  out  with  a  range  of  commanded  velocities  including 
any  velocity  with  a  y  component  less  than  or  equal  to  zero  (we  diagram  ranges 
of  commanded  velocities  as  sectors  of  a  circle).  These  are  the  velocities  that  will 
move  p  from  nearby  points  onto  the  goal  surface  G.  The  reachability  graph  for 
this  range  of  velocities  is  shown  at  the  top  of  Figure  10.  In  this  figure,  we  have 
indicated  those  surfaces  where  the  moving  object  may  stick  (using  the  electrical 
ground  symbol).  The  (potentially)  sticking  surfaces  are  those  whose  friction  cones 
overlap  the  current  velocity  range.  For  simplicity,  we  assume  that  the  contact  on 
surfaces  B  and  D  are  point  contacts. 

Figure  10  illustrates  the  reachability  graph  and  pre-image  of  the  goal  each 
time  Step  1  is  executed.  The  surfaces  used  to  constrain  the  range  of  commanded 
velocities  (Step  3)  were  chosen  in  the  following  order:  B,D,A,E.  The  particular 
order  does  not  affect  the  final  result  in  this  case.  The  algorithm  terminates  at  the 
fourth  cycle.  In  Figure  10(d),  we  have  shown  only  one  of  the  two  velocity  ranges 
(and  corresponding  P’s)  that  result  from  discarding  velocities  that  may  stick  on  A 
or  E.  The  remaining  velocity  range  leads  to  a  pre-image  that  is  the  mirror  image 
(about  the  hole  axis)  of  the  one  in  Figure  10(d).  Any  commanded  velocity  within 
either  of  these  remaining  velocity  ranges  will  reach  the  goal  from  any  position 
within  P.  Note  that  the  single  motion  strategy  developed  by  this  approach  is  a 
biased  search  (see  Section  1.1).  This  is  a  good  choice  since  we  have  not  included 
chamfers  or  rotation  in  our  problem  definition. 

The  example  above  can  be  done  with  a  single  motion  (because  the  friction 
cones  of  the  horizontal  and  vertical  surfaces  do  not  overlap).  We  did  not  require  the 
use  of  backward-chaining.  After  Step  2  of  the  algorithm  we  have  a  choice  of  refining 
the  range  of  directions  or  of  using  the  current  P  as  the  goal  for  a  recursive  call  to  the 
same  algorithm.  In  principle,  we  can  follow  these  two  paths  non-deterministically. 
In  practice,  this  requires  a  search  guided  by  considerations  such  as:  the  number  of 
motions  in  the  strategy  thus  far  and  the  size  of  P. 

The  example  in  Figure  11  illustrates  the  use  of  backward-chaining  to  develop 
multi-move  strategies.  In  this  example,  we  assume  that  the  first  four  steps  proceed 
essentially  as  in  the  example  in  Figure  10.  The  final  pre-image  of  the  first  example 
now  becomes  G1,  the  goal  for  the  next  recursion  level.  The  method  applies  as  before 
and  generates  a  new  pre-image  and  velocity  range.  The  strategy,  then,  consists  of 
choosing  some  velocity  from  this  tange,  moving  until  transition  into  G1  is  detected, 
and  changing  the  commanded  velocity  to  one  of  those  from  the  range  obtained  in 
the  first  example. 

We  noted  earlier  that,  for  each  commanded  motion  in  a  strategy,  it  is  necessary 
to  define  a  predicate  which  indicates  that  the  goal  has  been  reached.  In  multi-move 
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strategies,  this  condition  signals  that  another  motion  should  be  commanded.  Three 
types  of  basic  termination  conditions  are  available: 

1.  Position  termination:  Terminate  if  the  measured  position  of  p  is  such 
that  all  possible  actual  positions  consistent  with  the  measurement  are 
within  the  goal  region. 

2.  Velocity  (force )  termination:  Terminate  if  the  observed  velocity  of  p  is 
such  that  all  possible  actual  velocities  consistent  with  the  measurement 
can  only  occur  within  the  goal  region.  Note  that  since  motions  are 
generated  by  a  generalized  damper,  the  difference  between  actual  velocity 
and  commanded  velocity  provides  information  about  reaction  forces,  e.g., 
contact  with  a  surface. 

3.  Time  termination:  Terminate  if  the  elapsed  time  is  such  that  all  positions  , 
consistent  with  the  commanded  motion  and  observed  data  are  within  the 
goal. 

Position  termination  requires  that  all  actual  positions  consistent  with  the 
measured  position  be  within  the  goal.  This  is  equivalent  to  the  measured  position 
being  in  the  goal  after  shrinking  it  by  ec  along  its  boundary  to  free  space.  If  any 
of  the  dimensions  of  the  goal  region  are  less  than  ec,  then  position  feedback  is  not 
a  reliable  indicator  of  reaching  the  goal.  When  the  goal  is  a  surface,  for  example, 
shrinking  will  cause  the  goal  region  to  vanish.  In  these  cases,  we  must  rely  on 
velocity  termination  which  requires  that  the  observed  velocities,  e.g.,  when  landing 
on  or  leaving  from  a  surface,  be  unambiguous  relative  to  surfaces  that  may  be 
confused  with  the  goal  due  to  position  measurement  error.  Time  termination  is  also 
useful,  when  applicable,  as  it  is  much  simpler  to  test  than  position  termination. 

Velocity  termination  is  the  most  useful  termination  condition  when  faced  with 
large  position  uncertainty.  The  strategy  synthesized  from  the  example  in  Figure  11 
illustrate  this.  The  first  motion  required  by  the  strategy  can  be  terminated  when 
the  x  component  of  observed  velocity  is  zero,  i.e.,  when  p  strikes  one  of  walls  on  the 
right  of  the  hole.  The  second  motion  can  be  terminated  when  either  the  x  and/or 
y  components  of  the  observed  velocity  are  zero,  i.e.,  when  p  is  at  the  left  bottom 
corner  of  the  hole. 

These  two  examples  illustrate  the  class  of  fine-motion  strategies  we  wish  to 
consider.  The  strategies  operate  over  a  wide  range  of  uncertainty  without  explicitely 
computing  where  the  parts  are  relative  to  each  other.  The  strategics  do  not  keep 
any  explicit  history  of  previous  events  although,  we  will  see  later,  history  is  implicit 
in  the  strategies. 

2.6.  Configuration  Space 

The  basic  operation  in  the  synthesis  method  described  above  is  computing 
the  strong  pre-image  of  a  goal.  To  do  this,  we  first  transform  the  input  problem, 
involving  a  moving  object  and  stationary  obstacles,  into  an  equivalent  problem 
involving  a  point  and  transformed  obstacles.  This  transformation  has  a  number 
of  advantages.  One  is  that  it  enables  us  to  represent  the  pre-images  as  areas  in 


16 


Automatic  Synthesis  of 


Fine-Motion  Strategies  for  Robots 


Figure  12.  C-space  representations  make  motion  constraints  explicit 
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the  transformed  space.  The  key  advantage,  however,  is  that  this  transformation 
makes  the  constraints  on  motion  explicit.  This  is  illustrated  in  Figure  12  where 
an  upright  peg  and  chamfered  hole  are  shown  to  lead  to  transformed  obstacles 
similar  to  those  of  a  chamfered  peg  and  unchamfered  hole  and  to  those  of  a  tilted 
peg  and  unchamfered  hole.  The  transformation  has  served  to  make  explicit  the 
underlying  similarity  of  motion  constraints  in  these  tasks.  In  fact,  the  transformation 
reduces  tasks  involving  “arbitrary”  geometric  interactions  between  objects  to  the 
interactions  possible  between  a  point  and  a  set  of  surfaces. 

We  have  in  this  paper  limited  ourselves  to  two-dimensional  translation.  It  is 
possible,  however,  to  extend  the  transformation  approach  to  more  general  motions 
using  the  configuration  space  of  a  task  [Arnold  80,  Lozano-Perez  81,  83].  A 
configuration  of  an  object  is  the  set  of  parameters  needed  to  completely  specify 
the  position  of  all  points  of  the  object.  The  configuration  of  a  rigid  two-dimensional 
object,  for  example,  can  be  specified  by  two  displacements  and  an  angle,  that  of 
a  rigid  three-dimensional  object  by  three  displacements  and  three  angles,  and  that 
of  a  robot  arm  by  its  joint  angles.  For  concreteness,  we  will  be  dealing  exclusively 
with  cartesian  configurations,  e.g.,  ( x,y,9 )  for  objects  in  the  plane,  and  not  joint 
angle  configurations.  The  space  of  all  possible  configurations  for  an  object  is  known 
as  the  configuration  space  (C-space)  of  that  object.  An  object  A  is  represented  as  a 
point  in  its  C-space;  the  coordinates  of  that  point  are  the  configuration  parameters 
of  A. 

Stationary  obstacles  in  the  environment  of  a  moving  object  A  can  be  mapped 
into  the  configuration  space  of  A.  The  resulting  C-space  obstacles  are  those 
configurations  of  A  which  would  lead  to  collisions  between  A  and  the  obstacles. 
Configurations  on  the  surface  of  the  C-space  obstacle  due  to  B  are  those  where  some 
surface  of  A  is  just  touching  a  surface  of  B.  If  A  and  B  are  both  three-dimensional 
polyhedra,  the  surfaces  of  the  C-space  obstacle  for  B  arise  from  each  of  the 
feasible  contacts  between  of  vertices,  edges,  and  faces  of  A  and  B  (see  Figure  13) 
[Lozano-P£rez  83].  Therefore,  each  face  of  a  C-space  obstacle  represents  a  particular 
type  of  geometric  constraint  on  A.  A  range  of  positions  (and  orientations)  of  A  can 
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Figure  14.  Cross  sections  of  peg-in-hole  C- surfaces:  (a)  no  chamfer  (b)  chamfer 


be  represented  as  a  volume  in  the  C-space  of  A  and  a  motion  of  A  is  a  curve  in  the 
C-space. 

As  an  illustration  of  the  use  of  C-space  surfaces,  consider  the  familiar 
two-dimensional  peg-in-hole  problem  from  Figure  3.  We  can  construct  a  three- 
dimensional  C-space  of  ( x,y,9 )  configurations  of  the  peg.  In  this  space,  the  hole 
defines  an  obstacle  (see  Figure  14(a)).  Note  that  although  the  resulting  surfaces  are 
curved,  for  each  value  of  0  the  (i,  y)  cross  section  of  the  C-space  surfaces  is  polygonal. 
The  surfaces  represent  one-point  contacts  and  the  edges  at  the  intersections  of 
surfaces  represent  two-point  contacts.  Line-line  contacts  also  give  rise  to  edges  at 
the  intersections  of  one-point  contact  surfaces.  Figure  14(b)  shows  cross  sections 
for  a  peg  and  chamfered  hole. 

The  C-space  representation  can  be  extended  to  more  general  kinematic 
situations.  In  general,  motion  subject  to  geometric  and  kinematic  constraints  can 
be  defined  as  collections  of  equalities  and  inequalities  that  must  hold  among  the 
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parameters  that  determine  the  configurations  of  the  robot  and  the  objects  in  the 
task.  These  inequalities  represent  C-surfaces  [Mason  81].  Take  the  constraint  that 
a  robot  hand  remain  in  contact  with  a  crank  handle  as  it  rotates.  The  constraint 
relating  the  position  of  the  hand,  (x,y),  to  the  position  of  the  crank  (a  constant) 
and  its  current  angle,  a,  is  a  curve  (one-dimensional  surface)  in  the  configuration 
space  of  the  task,  i.e.,  the  ( x ,  y,  a)  space. 

Our  goal  is  to  make  the  detailed  analysis  of  assembly  operations  algorithmic  by 
casting  it  in  terms  of  C-surfaces.  The  purely  geometric  aspects  of  the  analysis  have 
been  exploited  in  earlier  work  on  obstacle  avoidance  [Brooks  and  Lozano-Perez 
83,  Lozano-Perez  81,  83j.  C-surfaces  also  share  many  of  the  characteristics  of 
“real”  surfaces  with  respect  to  force  analyses.  This  was  exploited  by  Mason  [81]  to 
synthesize  compliant  motions.  The  synthesis  approach  described  here  also  requires 
a  mechanism  for  computing  the  effects  of  friction.  Recent  work  has  developed  a 
definition  of  friction  cones  for  C-surfaces  [Erdmann  83).  Work  is  under  way  to  show 
that  conditions  for  avoiding  jamming  for  the  peg-in-hole  can  be  re-stated  in  terms 
of  the  relationship  of  applied  forces  to  these  C-space  friction  cones. 


3.  A  General  Framework 

In  the  previous  section  we  illustrated  an  abstract  planning  algorithm  for 
fine-motion  strategies.  Although  that  algorithm  is  representative  of  our  approach 
to  fine-motion  synthesis,  it  is  not  the  most  general  formulation  of  the  approach. 
In  particular,  that  algorithm  embodies  a  restrictive  assumption  on  the  class  of 
single-motion  strategies.  It  only  considers  strategies  obtained  by  discarding  all 
velocity  vectors  that  point  into  the  friction  cones  of  some  subset  of  the  task 
surfaces.  In  some  cases,  further  restrictions  of  the  class  of  velocity  vectors  would 
produce  a  better  strategy.  The  algorithm  of  the  previous  section  does  not  provide 
a  mechanism  for  further  restricting  the  range  of  velocities.  More  significantly,  we 
have  not  provided  a  criterion  for  defining  what  a  “better”  strategy  might  be. 

In  this  section  we  will  present  a  more  general  framework  for  our  approach  to 
fine-motion  synthesis.  Although  the  description  of  this  approach  takes  the  form 
of  an  algorithm,  it  is  not  detailed  enough  to  be  considered  an  effective  procedure. 
Our  goal  here  is  to  formulate  the  correctness  conditions  for  a  class  of  synthesis 
algorithms.  This  framework  can  be  used  to  elucidate  to  what  extent  particular 
synthesis  methods  (for  the  class  of  fine-motion  strategies  we  are  considering)  are 
“optimal.”  In  particular,  we  are  interested  in  strategies  that  make  the  best  possible 
use  of  sensory  data. 

Development  of  the  general  framework  begins  with  a  description  of  the  form 
of  termination  predicates  for  motions,  followed  by  a  discussion  of  the  pre-image 
definition,  and  of  the  necessity  of  passing  multiple  subgoals  to  recursive  calls  of  the 
planner.  The  rest  of  the  section  consists  of  a  formal  description  of  the  framework, 
expressed  as  an  abstract  algorithm,  and  an  extended  example  of  its  application  to 
the  peg-in-ho!e  problem  of  section  2. 
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3.1.  Termination  Predicates 

Much  of  the  burden  of  interpreting  uncertain  information  falls  on  the 
termination  predicate,  which  must  decide  when  the  current  goal  has  been  achieved. 
It  is  obviously  important  that  termination  not  be  premature;  otherwise  subsequent 
motions  will  proceed  on  a  false  assumption.  On  the  other  hand,  failure  to 
terminate  the  motion  when  the  goal  is  demonstrably  attained  is  also  bad;  the 
missed  opportunity  could  prevent  successful  completion  of  the  task.  Hence  it  is 
important  that  the  termination  predicate  make  the  best  possible  use  of  the  available 
information. 

One  restriction  is  placed  on  the  form  of  the  termination  predicates:  we  will 
exclude  predicates  which  record  sensory  data  for  later  use.  The  decision  to  terminate 
the  motion  must  be  made  based  on  current  sensor  readings  alone.  As  we  shall 
see,  there  is  another  mechanism  which  encodes  some  history,  so  this  constraint  is 
not  as  debilitating  as  it  may  first  appear.  If  later  developments  suggest  that  this 
restriction  should  be  relaxed,  the  framework  can  be  modified  by  allowing  a  state 
function  to  be  defined  along  with  each  predicate. 

The  form  of  the  termination  predicate  will  be  introduced  first  with  the 
assumption  of  perfect  sensing  and  control.  Consider  the  situation  just  after  a 
command  has  been  issued.  Given  perfect  knowledge  of  the  initial  position  and  a 
perfect  controller,  and  assuming  good  dynamic  models,  the  planner  could  predict 
the  subsequent  trajectory  of  the  robot.  If  the  position  and  force  sensors  were  perfect, 
it  would  be  a  simple  matter  to  watch  the  sensors,  or  the  time,  and  halt  the  motion 
when  the  robot  reaches  a  goal. 

To  address  more  realistic  problems,  we  will  first  relax  the  assumption  of  perfect 
sensing.  The  planner  still  knows  what  trajectory  the  robot  will  follow,  but  the 
sensing  information  cannot  be  taken  at  face  value.  It  is  necessary  to  construct  an 
interpretation  of  the  sensory  data,  which  will  be  the  set  of  all  positions/velocities 
consistent  with  the  sensory  data  and  with  the  trajectory.  Once  this  is  accomplished, 
termination  is  again  simple — if  this  interpretation  of  the  sensory  data  is  a  subset 
of  a  goal,  the  motion  is  terminated. 

The  final  step  is  to  relax  the  assumption  of  perfect  control  and  known  initial 
position.  Suppose  we  have  a  set  of  possible  initial  positions,  and  a  set  of  possible 
nominal  velocities.  Each  different  combination  of  initial  position  and  nominal 
velocity  will  give  a  different  robot  trajectory.  Without  knowing  which  trajectory  is 
the  “real”  one,  the  predicate  must  terminate  the  motion  with  a  guarantee  of  being 
in  a  goal.  To  see  how  this  is  done,  imagine  that  there  is  a  different  robot  for  each 
trajectory,  i.e.  that  all  of  the  trajectories  are  being  executed  simultaneously.  For 
each  robot,  we  can  apply  the  procedure  of  the  previous  paragraph:  form  the  set 
of  positions/velocities  consistent  with  sensory  data,  intersect  with  the  trajectory, 
check  for  inclusion  in  a  goal.  If  the  robots  all  agree  that  a  goal  has  been  achieved, 
the  motion  is  terminated.  This  approach  guarantees  that  for  any  initial  position 
and  nominal  velocity  consistent  with  the  robot’s  information,  and  for  any  position 
and  velocity  consistent  with  observations,  termination  will  occur  only  if  a  goal  is 
attained. 
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Figure  15.  Some  history  is  required  to  proceed  into  the  hole 


When  the  termination  predicate  is  constructed,  it  is  important  to  bring  all 
possible  information  to  bear,  so  that  the  set  of  “virtual  robots”  may  be  made  as 
small  as  possible.  Thus  far  we  have  concentrated  on  the  information  encoded  in 
the  sensory  data,  but  there  is  another  important  source  of  information.  When  we 
formulate  a  subgoal  R  and  call  the  planner  recursively,  there  are  two  important 
effects.  First,  the  robot  will  be  moved  to  R.  But  second,  and  more  to  the  point, 
is  that  when  the  recursive  call  returns  and  the  motion  is  executed,  the  planner 
knows  that  the  robot  is  in  R.  To  illustrate  the  distinction,  consider  a  robot  lightly 
touching  a  vertical  wall,  and  suppose  that  the  subgoal  R  is  the  wall.  Although  the 
robot  is  in  R,  this  fact  might  not  be  apparent  to  the  robot  if  the  contact  force  is 
small  and  if  the  position  sensors  are  noisy.  Hence  the  planner  is  called  recursively 
to  “move”  the  robot  to  the  wall.  Presumably  the  planner  will  plan  a  horizontal 
motion  into  the  wall.  When  the  motion  command  is  executed  the  robot  will  not 
move,  which  the  termination  predicate  will  interpret  as  evidence  that  the  “motion” 
was  successful.  When  the  recursive  invocation  of  the  planner  returns,  it  will  have 
accomplished  its  mission,  even  though  it  did  not  move  the  robot  at  all. 

Another,  more  familiar,  example  illustrates  the  use  of  this  information  to 
construct  the  termination  predicate.  Suppose  the  planner  is  applied  to  the  point- 
in-hole  problem,  with  the  position  sensor  giving  a  position  at  the  lip  of  the  hole 
(Figure  15).  Using  the  position  sensor  alone,  the  planner  would  have  to  admit 
the  possibility  of  the  robot  being  positioned  anywhere  inside  the  disc  centered  on 
the  sensed  position.  To  attain  the  goal  with  a  single  motion  would  be  impossible. 
However,  if  the  accomplished  subgoal  R  is  also  consulted,  the  set  of  possible  initial 
positions  is  reduced — the  robot  must  be  in  the  intersection  of  R  with  the  disc. 
Starting  from  this  smaller  set  of  possible  initial  positions,  with  a  command  nominal 
velocity  down  to  the  right,  it  is  easy  to  confirm  that  all  the  virtual  robots  will 
achieve  the  goal. 

Thus  the  history  of  the  robot,  represented  by  the  accomplished  subgoal  R, 
must  be  taken  into  account  to  construct  the  termination  predicate.  When  the  set 
of  feasible  trajectories  is  constructed,  initial  positions  outside  the  subgoal  R  should 
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be  excluded.  That  the  termination  predicate  is  dependent  on  the  accomplished 
subgoal  R  is  an  important  observation,  which  profoundly  affects  the  ultimate  form 
of  the  planning  algorithm. 

3.2.  Definition  of  Pre- image 

The  fundamental  element  in  our  approach  to  planning  is  the  ability  to  construct 
a  pre-image:  a  set  of  points  from  which  the  goal  can  be  attained  in  a  single  motion. 
In  section  2,  the  pre-image  depended  on  the  goal  G  and  a  range  of  command 
nominal  velocities.  By  proceeding  more  formally  in  this  section,  we  that  the 
pre-image  need  not  depend  on  the  command  nominal  velocities,  but  that  it  does 
depend  on  the  accomplished  subgoal  R. 

Conceptually,  we  can  approach  this  problem  as  follows:  For  goal  G,  and  for 
every  possible  observed  initial  position  cintt  and  accomplished  subgoal  R,  construct 
the  set  S(c*nit,  R,G)  of  all  command  nominal  velocities  such  that  the  termination 
predicate,  constructed  as  in  section  3.1,  is  guaranteed  to  terminate  the  motion.  If 
S(c’nit,R,  G)  is  empty,  there  is  no  single  motion  that  can  be  guaranteed  to  work 
for  accomplished  subgoal  R  and  observed  initial  position  cjmt.  If  S(cinit,  R,G)  is 
non-empty,  then  any  element  of  S(e*ntt,  R,  G)  is  sufficient  to  attain  the  goal.  Now 
if  the  actual  initial  position  of  the  manipulator  is  ctm(,  the  observed  initial  position 
c*mt  could  be  anywhere  in  the  sphere  B{cxnit)  centered  on  c,mt  with  radius  equal 
to  the  tolerance  on  the  position  sensor.  cxnit  should  be  in  the  pre-image  if  and  only 
if  every  possible  c*ntt  gives  a  non-empty  S(c’nit,  R,G).  Hence  we  can  define  the 
pre-image  Pr[G)  of  a  goal  G: 

Pr(G)  —  {c,ntt  €  R  |  V  ctn«t  £  B(Cintt)>  ctmt>  G)  7^  0}- 
The  subscript  R  is  used  as  a  reminder  that  the  pre-image  depends  on  R.  Note  also 
that  the  definition  of  Pr(G)  excludes  points  outside  R.  To  have  a  point  in  Pr(G) 
but  not  in  R  wouldn’t  make  much  sense — such  a  point  would  be  a  good  place  for 
the  robot  to  be,  provided  that  it  is  somewhere  else! 

3.3.  Recursive  Calls  and  Multiple  Goals 

When  the  planner  is  first  called,  the  robot  could  be  anywhere  in  configuration 
space  C.  If  the  set  of  strategies  guaranteed  to  attain  the  goal  G  from  any  point 
in  configuration  space  5(cjnit,C,  G)  is  non-empty,  then  the  planner  can  choose 
and  execute  one  of  these  strategies.  If  S(cintt,C,G)  is  empty,  the  planner  must 
construct  suitable  subgoals  and  initiate  a  recursive  call  to  the  planner  to  achieve 
these  subgoals. 

Clearly,  the  planner  should  specify  as  subgoals  only  those  sets  from  which  it 
can  achieve  the  goal,  otherwise  a  recursive  call  will  serve  no  purpose.  To  characterize 
these  subgoals  more  precisely,  let  us  look  ahead  a  bit,  and  imagine  that  the  recursive 
call  to  the  planner  has  just  returned.  The  recursive  call  guarantees  that  the  the 
robot’s  position  is  now  in  R.  Thus  the  planner  must  plan  a  single  motion,  from  initial 
position  Cinn  in  R,  which  attains  the  goal  G.  By  construction  of  the  pre-image, 
such  a  motion  exists  only  if  c,ni£  is  in  the  pre-image  Pr[G).  This  observation  serves 
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to  define  suitable  subgoals — R  should  include  only  those  points  which  are  also  in 
Pr[G),  i.e.  R  C  Pr[G).  Since,  by  construction,  R  D  Pr(G],  we  can  restate  this 
observation:  R  is  a  suitable  subgoal  if  and  only  if  R  —  Pr{G). 

Thus,  any  set  satisfying  the  equation  R  ==  Pr(G)  is  a  suitable  subgoal.  In 
general,  there  are  a  multitude  of  sets  which  satisfy  this  equation.  For  instance,  if  R 
satisfies  the  equation,  so  does  any  subset  of  R.  The  question  is  what  to  do  with  this 
multitude  of  subgoals.  Do  we  pass  them  to  recursive  calls  one  at  a  time?  Needless 
to  say,  the  branching  factor  in  this  search  can  be  rather  large.  However,  another 
issue  takes  precedence.  Situations  occur  for  which  the  planner  can  be  certain  to 
attain  one  of  two  goals,  but  cannot  be  certain  in  advance  which  of  the  two  goals 
it  will  attain.  If  the  planner  were  passed  either  one  of  the  two  goals  individually,  it 
would  fail  to  find  a  predicate  guaranteed  to  terminate  the  motion.  With  both  goals 
in  hand  simultaneously,  it  can  plan  a  motion  with  confidence  that  it  will  ultimately 
be  able  to  report  which  of  the  goals  was  attained.  Hence  we  will  pass  all  subgoals  to 
the  recursive  call.  This  suggests  that  the  approach  be  implemented  without  search, 
but  we  are  not  certain  whether  such  an  implementation  will  be  possible. 

Since  the  planner  will  be  passed  multiple  goals  rather  than  a  single  goal,  some 
adjustment  of  the  notation  is  required.  The  set  of  goals  will  be  written  {GQ},  the 
set  of  strategies  guaranteed  to  attain  one  of  the  goals  for  given  observed  initial 
position  c*nit  and  accomplished  subgoal  R  will  be  written  S(c'nit,R,  {Ga}),  and  the 
pre-image  will  be  written  H/?({Ga}). 

3.4.  A  Formal  Statement  of  the  FYamework. 

3.4.1.  Nomenclature 
c  configuration 

Cinu  configuration  at  beginning  of  a  motion 
v  velocity 
«o  nominal  velocity 
c  observed  configuration 

c*mt  observed  configuration  at  beginning  of  a  motion 
v*  observed  velocity 
i>q  commanded  nominal  velocity 
t  time 

C  C-space,  i.e.,  the  set  of  all  configurations 

B[c)  the  “uncertainty  ball”  of  configurations;  i.e.,  the  set  of  all  configurations  whose 
distance  from  c  is  within  the  tolerance  of  the  position  sensor. 

B(v )  the  “uncertainty  ball”  of  velocities. 

B(v0)  the  “uncertainty  ball”  of  nominal  velocities. 
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{G>}  current  goal  set.  We  wish  to  move  the  robot  to  one  of  the  goals  and  return 
the  identity  of  the  goal. 

p(c*,  v*,  t)  the  termination  predicate.  For  each  goal  in  { Ga }  it  returns  one  of:  “BUG”, 
indicating  that  no  possible  trajectory  is  consistent  with  any  interpretation  of  the 
sensory  data;  “CONTINUE”,  indicating  that  at  least  one  possible  trajectory 
exists,  consistent  with  the  sensory  data  not  just  at  the  goal;  or  “WIN", 
indicating  that  all  possible  trajectories  consistent  with  the  sensory  data  are  in 
the  goal. 

S[c*ntt,R,  { Ga })  is  the  set  {(vg,p(c*,u*,i))  |  p  terminates}.  By  construction  of  the 
predicates,  guaranteed '  termination  implies  guaranteed  attainment  of  a  goal. 
So  for  a  given  observed  initial  configuration  and  accomplished  subgoal  R,  this 
gives  the  set  of  all  winning  strategies,  where  a  strategy  comprises  a  command 
nominal  velocity  and  a  termination  predicate. 

PR{{Ga})  is  the  pre-image  {cinit  £R  |  V  c’nit  6  B(cxnit),  S(c’ntt,  R,  {GQ})  ^  0}. 

{f2^}  The  sets  of  configurations  R  such  that  the  pre-image  Pr({Gq})  includes  all 
of  R,  i.e.,  Pfl({Ga})  =  R.  This  is  the  subgoal  set — satisfaction  of  an  element 
of  this  set  by  a  recursive  call  will  allow  us  to  satisfy  the  current  goal  set. 

R  the  subgoal  attained  by  recursive  call  to  the  planner. 

Motor-Command  (oq)  execution  of  this  program  statement  transmits  the  commanded 
nominal  velocity  to  the  controller,  causing  the  manipulator  to  execute  the 
planned  generalized  damper  strategy. 

Dx<j(t)  the  actual  trajectory;  it  returns  (c,  v)  -  the  actual  configuration  and  velocity 
at  time  t  -  for  initial  position  and  nominal  velocity  Vo j. 

3.4.2.  Algorithm 

Procedure  FM({GQ}) 

Compute  {Rp} 

If  C  is  in  {/fy} 

Then  R  <-  C 
Else  R  <-  FM({flj8}) 

K,  P)  <"  choose  (S(c*nit,i?,{Ga») 
t  <-  0 

Motor Command (ug) 

L  {VQ}  <-  p{c,v\t ) 

ForEach  a  Do  If  Va  =  BUG  Then  Error 
ForEach  a  Do  If  Va  =  WIN  Then  Return  (Ga) 
Increment  t 
Go  L 
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Figure  16.  Task  illustrating  construction  of  p. 


3.5.  Example 

This  section  applies  the  algorithm  to  the  two-dimensional  peg-in-hole  problem 
(Figure  16).  FM  is  called  with  an  initial  goal  set  containing  the  single  element  G: 
the  bottom  of  the  hole.  Let  A,y(0  denote  an  actual  trajectory;  it  returns  (c,  v)  -  the 
actual  configuration  and  velocity  at  time  t  -  for  initial  position  ctnttlt  and  nominal 
velocity  v0ly. 

First  we  construct  an  example  illustrating  construction  of  the  termination 
predicate  p{c*,v  ,1).  Such  a  predicate  must  be  constructed  for  each  (c|mt, v0, /?). 
Here  is  the  predicate  for  (c*nt<2,  v<j|2,  R),  assuming  that  R  includes  J5(cintt  2). 

Procedure  P2,2(c*,  v*,  f) 

Flag  <-  False 

For  all  v0,y)  €  B(c-nit3)  X  H(vq  2) 

(c,v)  <-  Dij[t) 

If  (c,v)  <=  B(c*)  X  5(0 
Then  If  c  6  G 

Then  Flag  <-  True 
Else  Return (CONTINUE) 

If  Flag  Then  Return (WIN) 

Else  Return (BUG) 

S(c*nit,R,G)  is  the  set  of  all  (vq ,p)  which  are  guaranteed  to  win  if  executed  at  a 
point  in  2?(c*nit)  D  R ■  For  our  example  point,  this  set  is  empty.  For  example,  the 
command  nominal  velocity  Vq  2  is  pot  in  S(cinit,  R,  G)  because  trajectories  from  the 
left  and  right  edges  of  B(c‘ntt)  will  never  reach  the  goal. 

The  example  predicate  will  give  the  behavior  specified  in  sections  3.1  and  3.4, 
and  is  therefore  “correct”.  However,  the  form  of  the  predicates  is  not  completely 
satisfactory.  The  test  for  goal  attainment  is  done  simply  by  testing  whether  all 
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Figure  17.  Singleton  R’a 


possible  predicted  trajectories  have  attained  the  goal.  This  satisfies  the  requirements 
of  the  formalism  because  of  the  way  control  error  is  modeled — we  have  assumed 
that  the  control  error  is  constant  during  a  trajectory.  A  more  realistic  model  of 
error  would  yield  more  robust  predicates,  combining  position,  velocity,  and  time 
information  to  detect  presence  at  the  goal. 

The  first  step  in  the  algorithm  is  to  compute  a  set  of  subgoals  {Rp}.  Recall 
that  each  element  R  of  {Rp}  is  a  set  giving  a  pre-image  Pr({Gq})  which  is  equal 
to  R.  The  simplest  way  to  begin  is  to  construct  the  sets  consisting  of  a  single 
configuration.  Such  a  set  R  =  {c}  is  valid  if  and  only  if  it  gives  a  pre-image 
,P{ej({(7a})  equal  to  {c}.  Suppose  the  recursive  call  reports  the  manipulator  is  at  c, 
then  the  question  is  whether  a  single  motion  command  can  move  the  manipulator 
to  the  goal  G.  This  is  possible  for  all  configurations  c  in  the  shaded  region  of  Figure 
17.  This  region  is  the  union  of  two  half-planes  and  a  circular  disk  [Turk  1983].  A 
point  in  one  of  the  half-planes,  such  as  C3,  can  move  to  the  hole  by  selecting  a 
velocity  which  is  guaranteed  either  to  fall  to  one  side  of  the  hole  and  slide  in,  or  hit 
the  hole  directly.  A  point  in  the  circular  region,  such  as  c«,  can  move  to  the  hole 
by  selecting  a  velocity  which  is  guaranteed  to  hit  the  hole  directly. 

Any  set  R  satisfying  R  =  Ph({G0})  must  be  a  subset  of  the  region  indicated 
in  Figure  17.  However,  the  shaded  region  does  not  itself  constitute  a  good  R.  For 
instance,  as  in  the  earlier  example,  the  planner  might  be  unable  to  tell  whether 
the  robot  is  to  the  left  or  the  right  of  the  hole.  Hence  we  must  look  for  subsets 
R  of  the  shaded  region,  each  of  which  is  equal  to  the  corresponding  pre-image 
Rfl({Ga}).  Three  different  such  subsets  are  shown  in  Figures  18(a)  through  18(c). 
As  an  example,  consider  the  set  shown  in  Figure  18(c).  There  are  three  different 
regions  in  this  set:  one  region  to  the  left  of  the  hole,  one  region  to  the  right  of 
the  hole,  and  one  region  in  the  hole.  We  can  demonstrate  that  this  R  is  equal  to 
the  corresponding  Pr({Gq})  as  follows:  Suppose  that  a  recursive  call  has  reported 
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that  the  manipulator  is  in  R.  We  now  consult  the  position  sensor.  If  c‘mt  is  to  the 
right  of  center,  the  manipulator  cannot  be  in  the  left  region,  and  the  left-sliding 
command  shown  for  point  c 3  in  Figure  17  will  work.  Similarly  if  c’nit  is  to  the  left  of 
center,  a  right-sliding  motion  will  work.  This  set  works  because  any  “incompatible” 
subsets — the  left  and  right  regions  in  this  case — are  separated  by  a  distance  of  at 
least  2 ec. 

The  sets  shown  in  Figure  18  are  maximal — they  are  not  subsets  of  any  other 
subgoals.  Since  the  subset  of  any  valid  subgoal  is  itself  a  valid  subgoal,  it  would 
make  sense  to  pass  only  the  maximal  sets  to  the  recursive  call.  However,  situations 
do  occur  for  which  maximal  sets  do  not  exist,  so  we  will  simply  pass  all  valid 
subgoals  to  the  recursive  call. 

Once  the  subgoal  set  (Rfi)  has  been  determined  there  is  a  recursive  call  to 
FM.  As  in  section  2.1,  we  will  use  a  superscript  numeral  to  indicate  the  “recursion 
level”.  Thus  we  write  that  the  recursive  call’s  goal  set  is  the  original  call’s  subgoal 
set  by  writing  {G*}  <-  {7?$}.  Construction  of  a  predicate  for  a  multiple  goal  set 
is  a  simple  variation  of  the  predicate  constructed  earlier. 

Procedure  pi,i(c*,u*,f) 

Flag  <-  FALSE 

For  All  a  WinQ  <-  TRUE 

For  All  (c, t)0,i)  €  (R  fl  B(c*nit  l))  X  B{v'0A) 

(c,v)  <-  Dyj{t) 

If  (c,u)  6  B{c)  X  B{v) 
then  Flag  <-  TRUE 

For  All  a  If  c  £  Ga  Then  WinQ  <-  FALSE 
If  Flag  Then  If  For  Some  a  Wina  =  TRUE 
Then  Return (WIN) 

Else  Return (CONTINUE) 

Else  Return (BUG) 

Now  the  recursive  call  must  construct  subgoals.  In  this  case,  the  set  of  all 
configurations  C  is  a  valid  subgoal.  No  further  recursion  is  necessary,  because 
one  of  the  goals  can  be  attained  from  any  configuration  whatever  using  a  single 
motion  command.  The  recursive  call  may  immediately  choose  and  execute  a  motion 
command.  When  the  predicate  terminates  the  motion,  the  identity  of  the  subgoal 
attained  is  returned  to  the  original  call,  which  then  chooses  and  executes  a  motion 
command  carrying  the  manipulator  to  the  bottom  of  the  hole. 

4.  Conclusion 

This  paper  has  presented  a  formal  approach  to  the  synthesis  of  a  class  of 
fine-motion  strategies.  The  approach  operates  directly  from  geometric  descriptions 
of  the  task  and  explicit  bounds  on  errors  in  sensing  and  motion.  The  basic  method 
is  structured  around  the  computation  of  the  pre-image  of  a  goal  region,  i.e,  set  of 
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Figure  18.  Maximal  R’» 
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configurations  that  can  reach  the  goal  using  a  single  compliant  motion.  We  saw 
that  the  presence  of  errors  in  motion  and  sensing  gives  rise  to  a  number  of  difficult 
problems  in  specifying  motions  and  in  deciding  on  termination  conditions.  Further 
work  is  in  progress. 

Beyond  presenting  a  specific  synthesis  approach,  the  paper  has  attempted 

1.  to  illustrate  the  usefulness  of  modeling  compliant  fine-motion  strategies 
as  generalized  damper  motions  that  “slide”  on  C-surfaces  (corresponding 
to  geometric  constraints),  and 

2.  to  establish  correctness  conditions  for  fine-motion  programs  operating 
under  error  in  sensing  and  control. 

Our  approach  to  these  issues  provides  the  foundation  for  our  synthesis  method. 
Moreover,  we  hope  it  may  be  useful  to  human  programmers  engaged  in  fine-motion 
synthesis. 

The  approach  in  this  paper  is  part  of  an  attempt  to  develop  a  unified  approach 
to  robot  motion  planning,  spanning  obstacle  avoidance  (Brooks  and  Lozano-Perez 
83,  Lozano-Perez  81,  83],  compliant  motion  [Mason  81],  pushing  [Mason  82], 
grasping  [Lozano-Perez  81,  Mason  82],  and  (now)  fine-motion  strategies.  We  believe 
that  if  sophisticated  sensor-based  motion  strategies  are  to  be  routinely  used  in 
robotics,  the  analysis  and  synthesis  of  these  strategies  cannot  (or  should  not  have 
to)  be  done  by  human  programmers  on  a  task-by-task  basis.  Moreover,  we  are  in 
need  of  a  theoretical  basis  for  the  development  of  the  programming  and  control 
mechanisms  best  suited  for  sensor- based  motion.  For  these  reasons,  there  is  a  vital 
need  for  a  unified  (preferably  mechanizable)  approach  to  analysis  and  synthesis  of 
robot  motion.  This  paper  is  a  step  towards  this  goal. 

Appendix:  Compliance  via  Generalized  Damping 

Generalized  damping  is  a  very  simple  and  flexible  mechanism  for  implementing 
active  compliance3  [Whitney  77].  The  basic  approach  is  to  define  the  desired 
behavior  of  the  robot  by  the  following  relation: 

f  =  B(v  —  v0) 

where  f  is  the  vector  of  forces  acting  on  the  moving  object,  v0  is  the  nominal  velocity 
vector,  and  v  is  the  actual  velocity  vector.  In  general,  f  is  a  vector  of  six  cartesian 
forces  and  torques  and  v  and  vo  are  vectors  of  six  linear  and  rotational  velocities. 
In  our  examples  here,  we  limit  ourselves  to  forces  and  linear  displacements  in  the 
plane. 

Allowing  the  damping  matrix  B  to  be  an  arbitrary  matrix  can  produce 
unusual  behavior.  One  popular  example  is  to  relate  forces  in  the  — x  directions  to 
displacements  in  the  -f  y  direction  so  that  the  robot  will  climb  over  obstacles.  We 

3See  [Mason  83]  for  a  discussion  of  generaliied  damping  versus  the  generalised  stiffness  and 
hybrid  control  approaches  to  compliant  motion. 
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Figure  19.  Geometry  for  generalized  damper  analysis 


will,  however,  limit  ourselves  to  simple  damping  matrices.  In  particular,  we  assume 
B  to  be  the  a  diagonal  matrix  61,  with  6  >  0.  Note  that  the  damper  equation  ie 
now  simplified  to 


or  alternatively 


f  =  6(v  —  vq) 


f 


Consider  an  object  controlled  by  a  generalized  damper  with  vo  (at  an  angle 
6  below  horizontal)  on  a  rigid  surface  whose  normal  points  along  the  y  axis  (see 
Figure  19(a)).  When  the  object  strikes  the  surface,  three  possibilities  exist:  (1) 
the  object  slides  to  the  right;  (2)  the  object  slides  to  the  left;  or  (3)  the  object 
remains  motionless.  We  can  use  Coulomb’s  law  to  determine  which  of  these  three 
possibilities  will  occur. 

First  consider  case  (1);  the  object  slides  to  the  right,  so  the  velocity  v  is 
horizontal.  Coulomb’s  law  dictates  that  the  contact  force  f  will  make  an  angle 
j>  —  tan~lii  with  the  surface  normal.  Using  the  damper  equation  in  a  simple 
construction  in  velocity  space  (Figure  19(b))  we  see  that  the  nominal  velocity  angle 
Q  must  be  less  than  \  —  <f>.  Case  (2),  with  the  object  sliding  to  the  left  is  quite 
similar  and  yields  the  constraint  that  0  must  be  greater  than  |  0. 

Finally  consider  case  (3);  the  object  sticks,  i.e.  the  velocity  v  is  zero.  Coulomb’s 
law  gives  a  constraint  on  the  force: 

where  a  is  the  angle  which  the  force  f  makes  with  the  horizontal.  Again,  the  damper 
equation  implies  a  corresponding  constraint  on  0: 

<*-  +  *. 

The  analysis  above  yields  constraints  on  the  nominal  velocity  vq  given  the  motion 
of  the  object.  We  are  also  interested  in  the  opposite:  given  the  nominal  velocity  vq, 
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what  will  be  the  resultant  motion.  In  the  present  analysis  this  is  easily  obtained. 
If  the  nominal  velocity  angle  9  is  less  than  \  —  <f>,  only  case  (1),  right- sliding, 
can  occur.  If  the  nominal  velocity  angle  6  is  greater  than  f  -j-  <j>,  only  case  (2), 
left-sliding,  can  occur.  If  the  nominal  velocity  angle  9  is  in  the  interior  of  the  friction 
cone,  i.e.  if 

\~<t>  <9  <1+4, 

then  only  case  (3),  sticking,  can  occur.  The  only  ambiguous  cases  occur  when 
9  —  J  +  <t>  or  when  9  =  $  —  <f>.  These  cases  are  often  referred  to  as  “impending 
motion” . 

Thus  the  class  of  nominal  velocities  which  give  a  desired  motion  on  a  given 
surface  is  easily  characterized  in  terms  of  the  friction  cone  of  the  surface,  making 
the  generalized  damper  an  ideal  control  function  for  synthesis  of  fine  motions. 
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